#include "featurematch.h"

#include <iostream>
#include <cstdio>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/video/video.hpp>

#include "timestamp.h"

using namespace std;
using namespace cv;
FeatureMatch::FeatureMatch()
{
    this->m_match_cost = 0;
    this->m_detect_cost = 0;
    this->m_calc_cost = 0;
}


bool FeatureMatch::detectSift(Mat img, Rect object){
    TimeStamp ts;
    SIFT sift;
    Mat mask = img.zeros(img.size(), CV_8U);
    mask(object).setTo(1);
    return detectSift(img, mask, ts);
}

bool FeatureMatch::detectSift(Mat img, Mat mask, TimeStamp ts){
    SIFT sift;
    img.copyTo(this->m_temp_img);
    this->m_keypoints.clear();
    this->m_descriptors = Mat();
    sift(img, mask, this->m_keypoints, this->m_descriptors, false);
    if(this->m_keypoints.size() <= 0){
        fprintf(stderr, "detectSift lack keypoints\n");
        return false;
    }
    this->m_detect_cost = TimeStamp::now()->diff(ts);
    return true;
}

bool FeatureMatch::detectSurf(Mat img, Rect object){
    TimeStamp ts;
    Mat mask = img.zeros(img.size(), CV_8U);
    mask(object).setTo(1);
    return detectSurf(img, mask, ts);
}

bool FeatureMatch::detectSurf(Mat img, Mat mask, TimeStamp ts){
     SURF surf;
     img.copyTo(this->m_temp_img);
     this->m_keypoints.clear();
     this->m_descriptors = Mat();
     surf(img, mask, this->m_keypoints, this->m_descriptors, false);
     if(this->m_keypoints.size() <= 0){
         fprintf(stderr, "detectSurf lack keypoints\n");
         return false;
     }
     this->m_detect_cost = TimeStamp::now()->diff(ts);
     return true;
}

bool FeatureMatch::matchSift(Mat img, Rect area){
    TimeStamp ts;
    Mat mask = img.zeros(img.size(), CV_8U);
    mask(area).setTo(1);
    return matchSift(img, mask, ts);
}

bool FeatureMatch::matchSift(Mat img, Mat mask, TimeStamp ts){
    SIFT sift;
    vector<KeyPoint> keypoints;
    Mat descriptors;
    sift(img, mask, keypoints, descriptors, false);
    BFMatcher matcher(NORM_L1);
    vector<DMatch> matches;
    this->m_matches.clear();
    if(this->m_descriptors.empty() || descriptors.empty()){
        fprintf(stderr, "matchSift lack keypoints");
        return false;
    }
    matcher.match(this->m_descriptors, descriptors, matches);

    double min_dist = 10000, max_dist = 0;

    for (int i = 0; i < (int)matches.size(); i++){
        double dist = matches[i].distance;
        if (dist < min_dist) min_dist = dist;
        if (dist > max_dist) max_dist = dist;
    }

    this->m_matched_points.clear();
    for (int i = 0; i < (int)matches.size(); i++){
        if(matches[i].distance < 2 * min_dist){
            this->m_matches.push_back(matches[i]);
            this->m_matched_points.push_back(keypoints[matches[i].queryIdx]);
        }
    }

    drawMatches(this->m_temp_img, this->m_keypoints, img, keypoints, this->m_matches, this->m_match_img
//                , Scalar::all(-1), Scalar::all(-1),
//                vector<char>(), DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS
                );
   printf("all matches %u, good matches %u dist min %f, max %f\n", matches.size(), this->m_matches.size(), min_dist, max_dist);
    this->m_match_cost = TimeStamp::now()->diff(ts);
    return true;
}

bool FeatureMatch::matchSurf(Mat img, Rect area){
    TimeStamp ts;
    Mat mask = img.zeros(img.size(), CV_8U);
    mask(area).setTo(1);
    return matchSurf(img, mask, ts);

}

bool FeatureMatch::matchSurf(Mat img, Mat mask, TimeStamp ts){
    SURF surf;
    vector<KeyPoint> keypoints;
    Mat descriptors;
    surf(img, mask, keypoints, descriptors, false);

    cout<<"keypoints = "<<keypoints.size()<<"\n";
    BFMatcher matcher(NORM_L1);
    vector<DMatch> matches;
    this->m_matches.clear();

    if(this->m_descriptors.empty() || descriptors.empty()){
        fprintf(stderr, "matchSurf lack keypoints");
        return false;
    }
    matcher.match(this->m_descriptors, descriptors, matches);
    double min_dist = 10000, max_dist = 0;

    for (int i = 0; i < (int)matches.size(); i++){
        double dist = matches[i].distance;
        if (dist < min_dist) min_dist = dist;
        if (dist > max_dist) max_dist = dist;
    }

    for (int i = 0; i < (int)matches.size(); i++){
        if(matches[i].distance < 2 * min_dist){
            this->m_matches.push_back(matches[i]);
        }
    }
    drawMatches(this->m_temp_img, this->m_keypoints, img, keypoints, this->m_matches, this->m_match_img
//                ,
//                Scalar::all(-1), Scalar::all(-1),
//                vector<char>(), DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS
                );
    printf("all matches %u, good matches %u dist min %f, max %f\n", matches.size(), this->m_matches.size(), min_dist, max_dist);
    this->m_match_cost = TimeStamp::now()->diff(ts);
    return true;
}

bool FeatureMatch::calcColorFeature(Mat img, Rect obj){
    TimeStamp ts;
    int hsize = 16;
    int ch[] = {0, 0};
    float hranges[] = {0,180};
    const float* phranges = hranges;
    Mat hsv, hue;
    cvtColor(img(obj), hsv, CV_BGR2HSV);
    hue.create(hsv.size(), hsv.depth());
    mixChannels(&hsv, 1, &hue, 1, ch, 1);
    calcHist(&hue, 1, 0, Mat(), this->m_hist, 1, &hsize, &phranges);
    normalize(this->m_hist, this->m_hist, 0, 255, CV_MINMAX);

    this->m_calc_cost = TimeStamp::now()->diff(ts);
    return true;
}

bool FeatureMatch::detectOrb(Mat img, Mat mask, TimeStamp ts){
    ORB orb;
    img.copyTo(this->m_temp_img);
    this->m_keypoints.clear();
    this->m_descriptors = Mat();
    orb(img, mask, this->m_keypoints, this->m_descriptors, false);
    if(this->m_keypoints.size() <= 0){
        fprintf(stderr, "detectSift lack keypoints\n");
        return false;
    }
    this->m_detect_cost = TimeStamp::now()->diff(ts);
    return true;
}

bool FeatureMatch::matchOrb(Mat img, Mat mask, TimeStamp ts){
    ORB orb;
    vector<KeyPoint> keypoints;
    Mat descriptors;
    orb(img, mask, keypoints, descriptors, false);
    BFMatcher matcher(NORM_HAMMING);
    vector<DMatch> matches;
    this->m_matches.clear();
    if(this->m_descriptors.empty() || descriptors.empty()){
        fprintf(stderr, "matchSift lack keypoints");
        return false;
    }
    matcher.match(this->m_descriptors, descriptors, matches);

    double min_dist = 10000, max_dist = 0;

    for (int i = 0; i < (int)matches.size(); i++){
        double dist = matches[i].distance;
        if (dist < min_dist) min_dist = dist;
        if (dist > max_dist) max_dist = dist;
    }

    this->m_matched_points.clear();
    for (int i = 0; i < (int)matches.size(); i++){
        if(matches[i].distance < 2 * min_dist){
            this->m_matches.push_back(matches[i]);
            this->m_matched_points.push_back(keypoints[matches[i].queryIdx]);
        }
    }

    drawMatches(this->m_temp_img, this->m_keypoints, img, keypoints, this->m_matches, this->m_match_img
//                , Scalar::all(-1), Scalar::all(-1),
//                vector<char>(), DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS
                );
   printf("all matches %u, good matches %u dist min %f, max %f\n", matches.size(), this->m_matches.size(), min_dist, max_dist);
    this->m_match_cost = TimeStamp::now()->diff(ts);
    return true;
}

bool FeatureMatch::matchColor(Mat img, Rect &obj){
    TimeStamp ts;
    Mat hsv, hue;
    int ch[] = {0, 0};
    float hranges[] = {0,180};
    const float* phranges = hranges;
    cvtColor(img, hsv, CV_BGR2HSV);
    hue.create(hsv.size(), hsv.depth());
    mixChannels(&hsv, 1, &hue, 1, ch, 1);
    calcBackProject(&hue, 1, 0, this->m_hist, this->m_backkproj, &phranges);
    meanShift(this->m_backkproj, obj, TermCriteria( CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 10, 1 ));
//    obj = trackBox.boundingRect();
    this->m_match_cost = TimeStamp::now()->diff(ts);
    return true;
}
